package Controller;

import java.util.ArrayList;
import java.util.Hashtable;

import Interface.BluetoothNXT;

import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Button;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;

/**
 * The robot will drive a straight line until the underground is the default
 * color. Then, it will decide if it has crossed a barcode, and which one.
 * Finally, it will give instructions to the pilot according o the detected
 * barcode.
 */
public class BarcodeControlV2 {

	private static LightSensor sensor = new LightSensor(SensorPort.S1);
	private static TachoPilot pilot = new TachoPilot(2.20f, 6.30f, Motor.B,
			Motor.C);

	private static int marginLow = 11; //was 20
	private static Hashtable barcodeMap;
	private static int marginHigh = 30;
	private static ArrayList<Integer> readings = new ArrayList<Integer>();
	private static ArrayList<Integer> colors = new ArrayList<Integer>();

	private static ArrayList<String> recoverList = new ArrayList<String>();
	private static BluetoothNXT connection;
  	private static boolean bluetooth=false;
  	private static int speed=350;
	
	public static void main(String[] args) throws Exception {

//		sensor.setFloodlight(false);
		calibrateLightSensor();
		setSpeed();
		
		LCD.clear();
		LCD.drawString("Connect Bluetooth?", 0, 1);
		int button = Button.waitForPress();
		if (button == Button.ID_ENTER) {
			 connection = new BluetoothNXT(pilot, sensor,marginLow, marginHigh,
					new UltrasonicSensor(SensorPort.S2));
			connection.start();
			bluetooth=true;
		}
		LCD.clear();
		barcodeMap = new Hashtable();
		setupBarcodes();		
		recoverList.add("0010110"); //Up
		recoverList.add("0100101"); //Down
		recoverList.add("0011001"); //Left
		recoverList.add("0110011"); //Right
		recoverList.add("0001111"); //Straight

		try { testReadings(); } catch (InterruptedException e1) { }
		

		action();
	}

	public static void calibrateLightSensor() throws InterruptedException {

		Thread.sleep(1000);
		LCD.clear();
		LCD.drawString("Calibrating lightsensor", 0, 1);
		LCD.drawString("Set white value", 0, 2);
		//System.out.println("Set white value");
		Thread.sleep(1000);
		while (!Button.ENTER.isPressed()) {
			sensor.calibrateLow();
		}
		Thread.sleep(1000);
		LCD.drawString("Set black value", 0, 2);
		//System.out.println("Set black value");
		while (!Button.ENTER.isPressed()) {
			sensor.calibrateHigh();
		}
		LCD.clear();
		LCD.drawString("Lightsensor calibrated", 0, 1);
		LCD.drawString("High: " + sensor.getHigh(), 0, 2);
		LCD.drawString("Low: " + sensor.getLow(), 0, 3);
		LCD.drawString("Press enter to continue.", 0, 5);
		//System.out.println("Lightsensor calibrated");
		//System.out.println("High: " + sensor.getHigh() + " Low: "
		//		+ sensor.getLow());		
		//System.out.println("Press enter to continue.");

		Button.ENTER.waitForPress();

	}

	public static void action() {

		while (true) {
			//System.out.println("BarcodeReader");
			pilot.forward();
			 Motor.B.setSpeed(speed); 
			 Motor.C.setSpeed(speed);

			 
			 sleep(300); // sleep for cancelling initial bucking effects
			
			int counter = 0;

			while (counter < 3) {
				if (getColorFromValue(sensor.getLightValue()) != 2)
					counter++;
				else counter = 0;

			}

			ArrayList<Integer> readings = getBarcodeReading();

			if (readings != null) {

				String detectedBarcode = calculateBarcode(readings);
				Integer barcodeNb = (Integer) barcodeMap.get(detectedBarcode);
				if (barcodeNb != null)
				{
					LCD.drawString(""+barcodeNb, 0, 3);
					if(bluetooth)
						connection.setBarcode(barcodeNb);
					//System.out.println(barcodeNb);
				}
				else {
					barcodeNb = (Integer) barcodeMap
							.get(invertBarcode(detectedBarcode));
					if (barcodeNb == null)
					{
						System.out.println("null");
					}
					else
						pilot.rotate(190);
				}
				
				sleep(500);
				executeBarcode(barcodeNb, detectedBarcode);
				sleep(500);
			}
		}
	}

	/**
	 * Inverts the given string and returns the inversion
	 */
	private static Object invertBarcode(String detectedBarcode) {
		String ret = "";
		for (int i = detectedBarcode.length() - 1; i >= 0; i--) {
			ret += detectedBarcode.charAt(i);
		}
		return ret;
	}

	/**
	 * Returns whether the detected lightvalue is black (0), white(1) or
	 * neutral(2)
	 */
	private static int getColorFromValue(int value) {
		// Detected white
		if (value < 0 + marginLow){
//			System.out.println("0+marginlow: "+0+marginLow);
//			System.out.println("methode "+value);
			return 1;
		}
		// Detected black
		if(value>100-marginHigh){
			return 0;
		}
//		if (value > 100 - marginHigh && value<200){
//			return 0;
//		}
//		if(value>200){
//			System.out.println("lejos is stom");
//			return 2;
//		}
		else{
			return 2;
		}
	}

	/**
	 * Returns whether the detected lightvalue is black (0), white(1)
	 */
	private static int getColorFromValueBin(int value) {
		// Detected white
		if (value < 65) // 65
			return 1;
		// Detected black
		else
			return 0;
	}

	/**
	 * Transforms the data from the lightsensor into strings representing a
	 * barcode. Returns null if the crossed underground wasn't a barcode (ie it
	 * could be a sideline).
	 * 
	 * @throws InterruptedException
	 */
	private static ArrayList<Integer> getBarcodeReading() {
		readings.clear();

		int currentReading = 0;
		int neutralValueCount = 0;

		while (neutralValueCount < 250) {
			currentReading = sensor.getLightValue();
			//LCD.drawString(""+getColorFromValue(currentReading), 0, 4);
			System.out.println(getColorFromValue(currentReading));

			if (getColorFromValue(currentReading) == 2)
				neutralValueCount++;
			else
				neutralValueCount = 0;

			readings.add(currentReading);

			if (readings.size() > 800) {
				//LCD.drawString("Too long", 0, 5);
				System.out.println("Too long");
				return null;
			}

		}

		pilot.stop();

		// remove the last 250 (neutral) values from the readings
		for (int i = 0; i < 250; i++) {
			readings.remove(readings.size() - 1);
		}

		// prepare string with readings to print
		String output = "";
		for (int i = 0; i < readings.size(); i++) {
			output = output + " " + readings.get(i);
		}
		//System.out.println(output);
		//System.out.println("readings.size= " + readings.size());

		if (readings.size() < 150) {
			//LCD.drawString("Too short", 0, 5);
			System.out.println("Too short");
			return null;
		}
		
		return readings;
	}

	/**
	 * Returns a string which contains a barcode derived from given readings. It
	 * divides the readings in 7 equal blocks and calculates the mean of each
	 * block.
	 */
	private static String calculateBarcode(ArrayList<Integer> readings) {

		int barlength = Math.round(readings.size() / 7);
		colors.clear();

		for (int i = 0; i < 7; i++) {

			double tempsum = 0;
			for (int j = 0; j < barlength; j++) {
				tempsum += readings.get(i * barlength + j);
			}
			double meanValue = tempsum / barlength;

			colors.add(getColorFromValueBin((int) meanValue));
		}

		// prepare string to return
		String ret = "";
		for (int i = 0; i < colors.size(); i++) {
			int color = colors.get(i);
			ret = ret + color;
		}
		System.out.println("gelezen string: "+ret);
//		System.out.println("detected colors: ");
//		System.out.println(ret);

		return ret;
	}

	/**
	 * Executes the pilot-commands according to given barcodenumber.
	 */
	private static void executeBarcode(Integer bcNb, String bc) {
		if (bcNb == null) {
			//System.out.println("Invalid reading!");
			bcNb = 100;
		}

		// TODO insert correct values
		float distance = 10f;
		float angle = 93;
		switch (bcNb) {

		case 1: // straight
//			LCD.drawString("Straight", 0, 1);
			System.out.println("Straight!");
			pilot.travel(distance);	
			
			recoverList.clear();
			recoverList.add("0010110"); //Up
			recoverList.add("0100101"); //Down
			recoverList.add("0011001"); //Left
			recoverList.add("0110011"); //Right
			recoverList.add("0001111"); //Straight			
			break;
		case 3: // left turn
//			LCD.drawString("Left", 0, 1);
			System.out.println("Left!");
			pilot.travel(distance);
			pilot.rotate(angle);
			
			recoverList.clear();
			recoverList.add("0010110"); //Up
			recoverList.add("0100101"); //Down
			recoverList.add("0001111"); //Straight
			recoverList.add("0011001"); //Left
			recoverList.add("0110011"); //Right			
			break;
		case 4: // up
//			LCD.drawString("Up", 0, 1);
			System.out.println("Up!");
			pilot.travel(distance);
			
			recoverList.clear();			
			recoverList.add("0011001"); //Left
			recoverList.add("0110011"); //Right
			recoverList.add("0001111"); //Straight			
			break;
		case 5: // down
//			LCD.drawString("Down", 0, 1);
			System.out.println("Down!");
			pilot.travel(distance);
			
			recoverList.clear();			
			recoverList.add("0011001"); //Left
			recoverList.add("0110011"); //Right
			recoverList.add("0001111"); //Straight			
			break;
		case 6: // right turn;
//			LCD.drawString("Right", 0, 1);
			System.out.println("Right!");
			pilot.travel(distance);
			pilot.rotate(-angle);
			
			recoverList.clear();
			recoverList.add("0010110"); //Up
			recoverList.add("0100101"); //Down
			recoverList.add("0001111"); //Straight
			recoverList.add("0011001"); //Left
			recoverList.add("0110011"); //Right			
			break;
			
			
		case 100:
			//pilot.travel(-20);
			//recover(bc, recoverList, 0);
			//recover1(bc, recoverList, 0);
			recover2(bc, recoverList);
			//recover3(bc, recoverList);
			
			recoverList.clear();			
			
			recoverList.add("0011001"); //Left
			recoverList.add("0110011"); //Right	
			recoverList.add("0001111"); //Straight
			recoverList.add("0010110"); //Up
			recoverList.add("0100101"); //Down
			break;

		default:
			System.out.println("Detected bc doesnt respond to an action");
		}
	}
		
	/**
	 * Adds barcode-data to the barcodeMap See the online pdf for barcode
	 * numbers
	 */
	private static void setupBarcodes() {
		barcodeMap.put("0001111", 1); // straight
		barcodeMap.put("0110011", 6); // right turn
		barcodeMap.put("0011001", 3); // left turn
		barcodeMap.put("0010110", 4); // up
		barcodeMap.put("0100101", 5); // down		

	}

	private static void testReadings() throws InterruptedException {

		LCD.drawString("executing testreadings", 0, 1);
		//System.out.println("executing testreadings");
		while (!Button.ENTER.isPressed()) {

			//int reading = sensor.readValue();
			int reading = sensor.getLightValue();
			System.out.println("real: " +reading);
			System.out.println("fake: " +getColorFromValue(reading));
//			LCD.drawString("real: " +reading, 0, 3);
//			LCD.drawString("fake: " +getColorFromValue(reading), 0, 4);
			
//			System.out.println(getColorFromValue(reading) + " fake");
//			System.out.println(reading + " real");

		}
		LCD.clear();
	}

	private static void sleep(int millisec)
	{
		try {
			Thread.sleep(millisec);
		} catch (InterruptedException e) {
			System.out.println("Sleep exception");
		}		
	}
	
	private static void recover(String bc, ArrayList<String> rec, int level)
	{	
		LCD.drawString("Recover " +level, 0, 1);
		//System.out.println("Recover " +level);
		sleep(1000);
		
		ArrayList<String> recList = new ArrayList<String>();
		
		for(int i = 0; i<rec.size(); i++)
		{
			if(rec.get(i).charAt(level) == bc.charAt(level))
				recList.add(rec.get(i));
		}
		
		if (rec.size() == 1)
		{
			Integer execute = (Integer) barcodeMap.get(rec.get(0));
			executeBarcode(execute, bc);
			return;
		}		
		if (recList.size() == 0)
		{
			Integer execute = (Integer) barcodeMap.get(rec.get(0));
			executeBarcode(execute, bc);
			return;
		}
		
		recover(bc,recList, level+1);
	}
	
	private static void recover1(String bc, ArrayList<String> rec, int level)
	{	
		LCD.drawString("Recover " +level, 0, 1);
		//System.out.println("Recover " +level);
		sleep(1000);
		
		ArrayList<String> recList = new ArrayList<String>();
		
		for(int i = rec.size(); i<rec.size(); i++)
		{
			if(rec.get(i).charAt(rec.size()-1-level) == bc.charAt(rec.size()-1-level))
				recList.add(rec.get(i));
		}
		
		if (rec.size() == 1)
		{
			//Button.ENTER.waitForPress();
			Integer execute = (Integer) barcodeMap.get(rec.get(0));
			executeBarcode(execute, bc);
			return;
		}		
		if (recList.size() == 0)
		{
			//Button.ENTER.waitForPress();
			Integer execute = (Integer) barcodeMap.get(rec.get(0));
			executeBarcode(execute, bc);
			return;
		}
		
		recover(bc,recList, level+1);
	}
	
	private static void recover2(String bc, ArrayList<String> rec)
	{
		//LCD.drawString("Recovery 2", 0, 1);
		System.out.println("Recovery 2");
//		Button.ENTER.waitForPress();
		
		String recoveredBc = "";
		int minDist = 100;
		for(int i=0; i<rec.size();i++)
		{
			int hammingDist = getHammingDist(bc, rec.get(i));
//			int hammingDist = getHammingDistBlack(bc, rec.get(i));
			
			if( hammingDist < minDist)
			{
				minDist = hammingDist;
				recoveredBc = rec.get(i);
			}
		}
		Integer execute = (Integer) barcodeMap.get(recoveredBc);
		if(bluetooth)
			connection.setBarcode(execute);
		executeBarcode(execute, bc);
	}
	
	private static int getHammingDist(String s1, String s2)
	{
		int ret = 0;
		
		for(int i=0; i< s1.length(); i++)
		{
			if(s1.charAt(i) != s2.charAt(i))
				ret ++;
		}
		return ret;
	}
	
	private static int getHammingDistBlack(String read, String source)
	{
		int ret = 0;
		
		for(int i=0; i< source.length(); i++)
		{
			if(source.charAt(i) == 0 && read.charAt(i) != 0)
				ret ++;
		}
		return ret;
	}

	private static void recover3(String bc, ArrayList<String> rec)
	{
		LCD.drawString("Recovery 3", 0, 1);
		//System.out.println("recovery 3");
		sleep(1000);
		
		String recoveredBc = "";
		String middle = "";
		for(int i=0; i < rec.size();i++)
		{			
			middle = rec.get(i).substring(1, 4);
			
			int charcheck = 0;
			for(int j = 0; j< bc.length();i++)
			{
				if(bc.charAt(i) == middle.charAt(charcheck))
					charcheck ++;
				else
					charcheck = 0;
				if(charcheck == middle.length()-1)
				{
					LCD.drawString("" +rec.get(i), 0, 2);
					//System.out.println(rec.get(i));
					//Button.ENTER.waitForPress();
					executeBarcode((Integer) barcodeMap.get(rec.get(i)), bc);
					return;
				}
			}
				
		}
		// 0001100 0011001
		
	}
	private static void setSpeed() {
		LCD.clear();
		LCD.drawString("Maxspeed: " + speed + " degr/sec",0,2 );
		Buttonloop: {
			while (true) {				
				int button = Button.waitForPress();
				switch (button) {
				case 1:
					break Buttonloop;
				case 2:
					speed -= 50;
					if (speed < 50)
						speed = 50;
					break;
				case 4:
					speed += 50;
					break;				
				}
				LCD.drawString("Maxspeed: " + speed + " degr/sec",0,2 );
			}
		}
	}


}
